/**
 * @copyright (C) COPYRIGHT 2022 Fortiortech Shenzhen
 * @file      AddFunction.c
 * @author    Fortiortech  Appliction Team
 * @since     Create:2022-07-13
 * @date      Last modify:2022-07-14
 * @note      Last modify author is Marcel He
 * @brief     This file contains main function used for Motor Control.
 */

#include <MyProject.h>

/* Public variables --------------------------------------------------------- */

bool data isCtrlPowOn = false;           ///< 开关机控制
PWMINPUTCAL xdata mcPwmInput;            ///< PWM捕获结构体变量
FOCCTRL xdata mcFocCtrl;                 ///< FOC电机控制相关结构体变量
MCRAMP xdata mcRefRamp;                  ///< 控制指令爬坡结构体相关变量
debugONOFFTypeDef xdata debug_ONOFFTest; ///< ONOFF启停测试小工具结构体变量
SLEEPMODE xdata SleepSet;                ///< 休眠结构体变量
uint16 xdata Power_Currt;

/**
    @brief        对变量取32位的绝对值
    @param[in]    value
    @return       绝对值
    @date         2022-07-13
*/
uint32 Abs_F32(int32 value)
{
    if (value < 0)
    {
        return (-value);
    }
    else
    {
        return (value);
    }
}

/**
 * @brief        PWM调速信号计算，本例程提供Duty计算，如需频率信号可自行使用mcPwmInput.Period周期值计算
 * @date         2022-07-14
 */
extern uint8 TempeDerating;
void PWMDutyCal(void)
{
    static uint16 dutyTemp = 0;

    if (mcPwmInput.isUpdate) // 有新的duty更新
    {
        if ((Abs_F32(mcPwmInput.TimerDR - mcPwmInput.TimerDROld) < 0xFF)       // 误差在1个Byte之间再处理
            && (Abs_F32(mcPwmInput.TimerARROld - mcPwmInput.TimerARR) < 0xFF)) // 误差在1个Byte之间再处理
        {
            mcPwmInput.Compare = mcPwmInput.TimerDR; // 读取DR与ARR值
            mcPwmInput.Period = mcPwmInput.TimerARR;
            mcPwmInput.Duty = MDU_DIV0_GetQL16(mcPwmInput.Compare >> 1, 0x0000, mcPwmInput.Period);
/***速度随PWM增大而增大***/
#if (PWMDUTY_POLARITY == NegaPWMDUTY)
            {
                dutyTemp = 32768 - mcPwmInput.Duty;
            }
/***速度随PWM增大而减小***/
#else
            {
                dutyTemp = mcPwmInput.Duty;
            }
#endif

            if (dutyTemp > ONPWMDuty)
            {
                mcPwmInput.MotorOffFilter = MotorOffFilterTime;
                if (mcPwmInput.MotorOnFilter == 0) // 开机滤波
                {
                    mcPwmInput.MotorOnFilter = MotorOnFilterTime;
                    isCtrlPowOn = true; // 开机
                }
            }
            else if ((dutyTemp < OFFPWMDuty) || (dutyTemp > OFFPWMDutyHigh))
            {
                mcPwmInput.MotorOnFilter = MotorOnFilterTime;
                if (mcPwmInput.MotorOffFilter == 0) // 关机滤波
                {
                    mcPwmInput.MotorOffFilter = MotorOffFilterTime;
                    isCtrlPowOn = false; // 关机
                }
            }
            else
            {
                // 不做处理，保持前一个状态
            }

            // 转速曲线计算
            if (isCtrlPowOn == true)
            {
                if (dutyTemp <= MINPWMDuty)
                {
                    mcFocCtrl.Ref = MOTOR_SPEED_MIN_RPM;
                }
                else if (dutyTemp >= MAXPWMDuty)
                {
                    mcFocCtrl.Ref = MOTOR_SPEED_MAX_RPM;
                }
                else
                {
                    mcFocCtrl.Ref = MOTOR_SPEED_MIN_RPM + SPEED_K * (dutyTemp - MINPWMDuty);
                }
                if (TempeDerating) // 过温降低额度
                {
                    mcFocCtrl.Ref = mcFocCtrl.Ref >> 1;
                    if (mcFocCtrl.Ref < MOTOR_SPEED_MIN_RPM)
                        mcFocCtrl.Ref = MOTOR_SPEED_MIN_RPM;
                }
            }
            else
            {
                mcFocCtrl.Ref = 0;
            }
        }

        mcPwmInput.isUpdate = 0;
        mcPwmInput.TimerDROld = mcPwmInput.TimerDR;   // 将此次比较值赋值给上次比较值
        mcPwmInput.TimerARROld = mcPwmInput.TimerARR; // 将此次周期值赋值给上次周期值
    }
}

/**
 * @brief        调速信号处理包含：开关机控制、将调速信号处理成控制目标给定信号
 * @date         2022-07-14
 */
void TargetRef_Process(void)
{

    PWMDutyCal();
#if (SPEED_MODE == NONEMODE)
    {
        isCtrlPowOn = true; // 开机
        mcFocCtrl.Ref = MOTOR_SPEED_MIN_RPM;
    }
#endif
}

/**
 * @brief        外部闭环控制函数，示例代码提供 电流环，速度环，功率环，UQ控制示例代码,可根据需要自行修改
 *               建议使用默认1ms周期运行
 * @date         2022-07-14
 */
uint8 Flag_Single_Mode = 0;
uint16 drv = 0;
void Speed_response(void)
{
    static int16 refRampOut = 0;

    if (mcState == mcRun)
    {
        drv = DRV_DR;
        if (mcFocCtrl.Ref > mcFocCtrl.mcDcbusRef)
        {
            if (drv <= PWM_VALUE_LOAD)
                DRV_DR = drv + 2;
        }
        else
        {
            if (drv > 2)
                DRV_DR = drv - 2;
        }
    }
}

/**
 * @brief        启动ATO爬坡函数，用于静止启动时候对ATO进行爬坡，提高启动可靠性
 * @date         2022-07-14
 */
void ATORamp(void)
{
    if (mcState == mcRun)
    {
    }
}
/**
 * @brief        默认1ms周期服务函数，运行信号采样，调速信号处理，闭环控制，故障检测,ATO爬坡函数
 *               该函数运行于大循环中，由SYSTICK定时器间隔1ms触发运行。
 * @date         2022-07-14
 */
void TickCycle_1ms(void)
{
    SetBit(ADC_CR, ADCBSY); // 使能ADC的DCBUS采样

    if ((mcState != mcInit) && (mcState != mcReady))
    {
        /* -----速度滤波----- */
        mcFocCtrl.SpeedFlt = MDU_LPF0(FOC__EOME, mcFocCtrl.SpeedFlt, 50);   /* -----估算器估算的速度值滤波--- 注意低通滤波器系数范围为0---127 */
        mcFocCtrl.EsValue = MDU_LPF0(FOC__EMF << 2, mcFocCtrl.EsValue, 50); /* -----估算器估算的反电动势值滤波--- */

        if (mcState == mcRun)
        {
            mcFocCtrl.Power = FOC__POW << 1;
            /* -----功率滤波----- */
            mcFocCtrl.PowerFlt = LPFFunction(mcFocCtrl.Power, mcFocCtrl.PowerFlt, 30); /* -----功率值滤波----- */
        }
    }
    else
    {
        mcFocCtrl.SpeedFlt = 0;
        mcFocCtrl.PowerFlt = 0;
    }
    /* -----母线电流值采样----- */
    Power_Currt = (ADCCurrentbusPort);

    if (Power_Currt > mcCurOffset.Iw_busOffset)
    {
        Power_Currt = Power_Currt - mcCurOffset.Iw_busOffset;
    }
    else
    {
        Power_Currt = 0;
    }
    mcFocCtrl.mcADCCurrentbus = MDU_LPF0(Power_Currt, mcFocCtrl.mcADCCurrentbus, 32); /* -----母线电流值滤波----- */

    mcFocCtrl.mcDcbusFlt = MDU_LPF0(ADC2_DR, mcFocCtrl.mcDcbusFlt, 50); /* -----外部母线电压值滤波，匹配硬件端口----- */

    mcFocCtrl.NTCTempFlt = MDU_LPF0(ADC3_DR, mcFocCtrl.NTCTempFlt, 50); /* -----NTC电压值滤波----- */
    mcFocCtrl.UqFlt = MDU_LPF0(FOC__UQ, mcFocCtrl.UqFlt, 50);           /* -----Q轴电压值滤波----- */
    mcFocCtrl.UdFlt = MDU_LPF0(FOC__UD, mcFocCtrl.UdFlt, 50);           /* -----D轴电压值滤波----- */

    mcFocCtrl.MCU_TEMP = TSD_Gain(); // 获取芯片内部温度

    mcFocCtrl.mcDcbusMOTO2 = MDU_LPF0(ADC2_DR, mcFocCtrl.mcDcbusMOTO2, 50); /* -----NTC电压值滤波----- */
    mcFocCtrl.mcDcbusMOTO1 = MDU_LPF0(ADC1_DR, mcFocCtrl.mcDcbusMOTO1, 50); /* -----NTC电压值滤波----- */
    if (mcFocCtrl.mcDcbusMOTO2 > mcFocCtrl.mcDcbusMOTO1)
        mcFocCtrl.mcDcbusRef = mcFocCtrl.mcDcbusMOTO2 - mcFocCtrl.mcDcbusMOTO1;
    else
        mcFocCtrl.mcDcbusRef = 0;

    /* 获取调速信号，不同调速模式(PWMMODE,NONEMODE,SREFMODE,KEYSCANMODE)的目标值修改 */
    TargetRef_Process();
    /* 故障保护函数功能，如过欠压保护、启动保护、缺相、堵转等 */
    Fault_Detection();
    /* 启动ATO控制，环路响应，如速度环、转矩环、功率环等 */
    Speed_response();
    /* 电机启动ATO爬坡函数处理  */
    ATORamp();
/*****休眠********/
#if (SleepEn == Enable)
    sc_goToSleep(); // PWM信号输入悬空时或全低全高时进入休眠，悬空时PWM上拉到12V
#endif

    /* 电机状态机的时序处理 */
    if (mcFocCtrl.State_Count > 0)
    {
        mcFocCtrl.State_Count--;
    }
    if (mcPwmInput.MotorOnFilter > 0)
    {
        mcPwmInput.MotorOnFilter--;
    }
    if (mcPwmInput.MotorOffFilter > 0)
    {
        mcPwmInput.MotorOffFilter--;
    }
    if (EOP_RStime)
        EOP_RStime--;
}
